/*!
  @file camera_params.cpp
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#include "mrover_vo/camera_params.h"

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using cv::Mat;
using cv::Rect;

namespace vo {

void CameraParams::initFromFile(std::string &yaml) {   
  cv::FileStorage camera_yaml_fs(yaml, cv::FileStorage::READ);
  if (!camera_yaml_fs.isOpened()) {
    ROS_ERROR("Camera yaml file open falied");
  }

  Mat _wTl = wT[0](cv::Rect(0, 0, 4, 3));
  Mat _wTr = wT[1](cv::Rect(0, 0, 4, 3));

  camera_yaml_fs["Kl"] >> K[0];
  camera_yaml_fs["Kr"] >> K[1];
  camera_yaml_fs["Dl"] >> D[0];
  camera_yaml_fs["Dr"] >> D[1];
  camera_yaml_fs["baseline"] >> baseline;
  camera_yaml_fs["width"] >> image_size.width;
  camera_yaml_fs["height"] >> image_size.height;
  camera_yaml_fs["Tl"] >> _wTl;
  camera_yaml_fs["Tr"] >> _wTr;

  Mat rTl(4, 4, CV_32F);
  rTl = wT[1].inv() * wT[0];
  Mat _rTl = rTl(Rect(0, 0, 4, 3));
  P[0] = K[0] * Mat::eye(3, 4, CV_32F);
  P[1] = K[1] * _rTl;


  //-- debug
  {
    ROS_INFO_STREAM("Reading camera parameters");
    ROS_INFO_STREAM("--> Kl: " << K[0]);
    ROS_INFO_STREAM("--> Kr: " << K[1]);
    ROS_INFO_STREAM("--> Dl: " << D[0]);
    ROS_INFO_STREAM("--> Dr: " << D[1]);
    ROS_INFO_STREAM("--> Tl: " << wT[0]);
    ROS_INFO_STREAM("--> Tr: " << wT[1]);
    ROS_INFO_STREAM("--> Pl: " << P[0]);
    ROS_INFO_STREAM("--> Pr: " << P[1]);
    ROS_INFO_STREAM("--> baseline: " << baseline); 
    ROS_INFO_STREAM("--> image size: " << image_size.width << "x" << image_size.height);
  }

  boost::thread thread(boost::bind(&CameraParams::publishTF, this));
}

void CameraParams::publishTF() {
  tf::Transform tfL;
  convert(cv::Mat::eye(4, 4, CV_32F), tfL);

  tf::Transform tfR;
  Mat lTr(4, 4, CV_32F);
  lTr = wT[0].inv() * wT[1];
  convert(lTr, tfR);

  tf::Transform tf_base;
  convert(wT[0], tf_base);
  tf_base.setOrigin(tf::Vector3(0, 0, 0));
  tf_pub_.sendTransform(tf::StampedTransform(tf_base, ros::Time::now(), "/map", "/camera_frame"));

  while (true) {
    tf_pub_.sendTransform(tf::StampedTransform(tfL, ros::Time::now(), "/camera_frame", "/camera_left"));
    tf_pub_.sendTransform(tf::StampedTransform(tfR, ros::Time::now(), "/camera_left", "/camera_right"));

    boost::posix_time::seconds t(1.0);
    boost::this_thread::sleep(t);
  }
}

void CameraParams::convert(const cv::Mat &T, tf::Transform &transform) {
  tf::Vector3 position;
  tf::Matrix3x3 rotation;
  tf::Quaternion orientation;
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      rotation[i][j] = T.at<float>(i, j);
    }
    position[i] = T.at<float>(i, 3);
  }
  rotation.getRotation(orientation);
  transform.setRotation(orientation);
  transform.setOrigin(position);
}

}  // namespace vo
